﻿#include "pidfuc.h"

pidFuc::pidFuc(QObject *parent)
    : QObject{parent}
{}

void pidFuc::setPidTag(float p,float i,float d,float target,float maxOut)
{
    tag.value_p = p;
    tag.value_i = i;
    tag.value_d = d;

    tag.target = target;
    tag.outputLimit = maxOut;

    tag.sum_error = 0;
    tag.sum_errorLimit = 1000;

    tag.last_error = 0;

}
float pidFuc::pidControl(float value,float time){
    float error = tag.target - value;
    float p_term = tag.value_p*error;

    tag.sum_error +=error*time;
    if(tag.sum_error>tag.sum_errorLimit&&tag.sum_error>0)tag.sum_error = tag.sum_errorLimit;
    else if(tag.sum_error<tag.sum_errorLimit&&tag.sum_error<0)tag.sum_error = -tag.sum_errorLimit;

    float i_term = tag.value_i*tag.sum_error;

    float d_term;
    if(time>0) d_term = tag.value_d*(error-tag.last_error)/time;
    else d_term = 0;

    float output = p_term + i_term + d_term;
    if(abs(output)<tag.outputLimit)return output;
    else return tag.outputLimit*output/abs(output);

}
